#ifndef __Quaternion_hpp__
#define __Quaternion_hpp__

#include "Matrix3.hpp"

namespace ge {

//----------------------------------------------------------------------------//
// Quaternion
//----------------------------------------------------------------------------//

class Quaternion
{
 public:

     Quaternion( float _x = 0.0f, float _y = 0.0f, float _z = 0.0f, float _w = 1.0f );

     Quaternion( const Quaternion& _q );

     Quaternion( const Matrix3& _r );

     Quaternion( const Vector3& _axis, float _angle );

     Quaternion( const Vector3& _x, const Vector3& _y, const Vector3& _z );

     Quaternion( const Vector3 _axes[3] );

     Quaternion( const float _v[4] );

     Quaternion& operator = ( const Quaternion& _q );

     Quaternion& operator = ( const Matrix3& _m );

     const float& operator [] ( uint _index ) const;

		   float& operator [] ( uint _index );

     float* operator * ( void );

     const float* operator * ( void ) const;

     Quaternion operator + ( const Quaternion& _q ) const;

     Quaternion operator - ( const Quaternion& _q ) const;

     Quaternion operator * ( const Quaternion& _q ) const;

     Quaternion operator * ( float _s ) const;

     friend Quaternion operator * ( float _s, const Quaternion& _q );

     Quaternion operator - ( void ) const;

     const Quaternion& operator + ( void ) const;

     float Dot( const Quaternion& _q ) const;

     Quaternion& Inverse( void );

     Quaternion InverseCopy( void ) const;

     float Norm( void ) const;

     Quaternion UnitInverse( void ) const;

     Quaternion Exp( void ) const;

     Quaternion Log( void ) const;

     Vector3 operator * ( const Vector3& _v ) const;

     friend Vector3 operator * ( const Vector3& _v, const Quaternion& _q );

     Quaternion& Normalize( void );

     Quaternion NormalizedCopy( void );

	   	Quaternion& FromRotationMatrix( const Matrix3& _m );

     Matrix3& ToRotationMatrix( Matrix3& _m ) const;

     Matrix3 ToRotationMatrix( void ) const;

		   Quaternion& FromAxisAngle( const Vector3& _axis, float _angle );

     void ToAngleAxis( Vector3& _axis, float&_angle ) const;

     float Roll( bool _reproject_axis = true ) const;

		   float Pitch( bool _reproject_axis = true ) const;

		   float Yaw( bool _reproject_axis = true ) const;

     Vector3 AxisX( void ) const;

     Vector3 AxisY( void ) const;

     Vector3 AxisZ( void ) const;

     bool operator == ( const Quaternion& _q ) const;

     bool operator != ( const Quaternion& _q ) const;

     Quaternion& FromAxes( const Vector3& _x, const Vector3& _y, const Vector3& _z );

     void ToAxes( Vector3& _x, Vector3& _y, Vector3& _z ) const;

     bool Equals( const Quaternion& _q, float _tolerance = Math::ToRadians( Math::Epsilon ) ) const;

     Quaternion& Set( const Quaternion& _q );

     Quaternion& Set( float _x, float _y, float _z, float _w );

     Quaternion& FromPtr( const float _v[4] );

     Quaternion& SetZero( void );

     Quaternion& SetIdentity( void );

 public:

     static Quaternion Nlerp( float _t, const Quaternion& _p, const Quaternion& _q, bool _shortest_path = false );

     static Quaternion Slerp( float _t, const Quaternion& _p, const Quaternion& _q, bool _shortest_path = false );

     static Quaternion SlerpExtraSpins( float _t, const Quaternion& _p, const Quaternion& _q, int _extra_spins );

     static void Intermediate( const Quaternion& _q0, const Quaternion& _q1, const Quaternion& _q2, Quaternion& _a, Quaternion& _b );

     static Quaternion Squad( float _t, const Quaternion& _p, const Quaternion& _a, const Quaternion& _b, const Quaternion& _q, bool _shortest_path = false );
     
 public:

     static const Quaternion Zero;
     static const Quaternion Identity;
     
 public:

     union
     {
          struct { float x,y,z,w; };
          float v[4];
     };
};

// ???
namespace Math {

    template < class T > Quaternion Mix( const Quaternion& _a, const Quaternion& _b, const T& _time )
    {
         return Quaternion::Slerp( static_cast<float>(_time), _a, _b ); 
    }

} // namespace Math

//----------------------------------------------------------------------------//
// Quaternion
//----------------------------------------------------------------------------//

//----------------------------------------------------------------------------//
inline Quaternion::Quaternion( float _x, float _y, float _z, float _w ) : x( _x ), y( _y ), z( _z ), w( _w )
{
}
//----------------------------------------------------------------------------//
inline Quaternion::Quaternion( const Quaternion& _q ) : x( _q.x ), y( _q.y ), z( _q.z ), w( _q.w )
{
}
//----------------------------------------------------------------------------//
inline Quaternion::Quaternion( const Matrix3& _r )
{
     FromRotationMatrix( _r );
}
//----------------------------------------------------------------------------//
inline Quaternion::Quaternion( const Vector3& _axis, float _angle )
{
     FromAxisAngle( _axis, _angle );
}
//----------------------------------------------------------------------------//
inline Quaternion::Quaternion( const Vector3& _x, const Vector3& _y, const Vector3& _z )
{
     FromAxes( _x, _y, _z );
}
//----------------------------------------------------------------------------//
inline Quaternion::Quaternion( const Vector3 _axes[3] )
{
     FromAxes( _axes[0], _axes[1], _axes[2] );
}
//----------------------------------------------------------------------------//
inline Quaternion::Quaternion( const float _v[4] ) : x( _v[0] ), y( _v[1] ), z( _v[2] ), w( _v[3] )
{
}
//----------------------------------------------------------------------------//
inline Quaternion& Quaternion::operator = ( const Quaternion& _q )
{
     x = _q.x;
     y = _q.y;
     z = _q.z;
     w = _q.w;
     return *this;
}
//----------------------------------------------------------------------------//
inline Quaternion& Quaternion::operator = ( const Matrix3& _m )
{
     return FromRotationMatrix( _m );
}
//----------------------------------------------------------------------------//
inline const float& Quaternion::operator [] ( uint _index ) const
{
     return v[_index];
}
//----------------------------------------------------------------------------//
inline float& Quaternion::operator [] ( uint _index )
{
     return v[_index];
}
//----------------------------------------------------------------------------//
inline float* Quaternion::operator * ( void )
{
     return v;
}
//----------------------------------------------------------------------------//
inline const float* Quaternion::operator * ( void ) const
{
     return v;
}
//----------------------------------------------------------------------------//
inline Quaternion Quaternion::operator + ( const Quaternion& _q ) const
{
     return Quaternion( x + _q.x, y + _q.y, z + _q.z, w + _q.w );
}
//----------------------------------------------------------------------------//
inline Quaternion Quaternion::operator - ( const Quaternion& _q ) const
{
     return Quaternion( x - _q.x, y - _q.y, z - _q.z, w - _q.w );
}
//----------------------------------------------------------------------------//
inline Quaternion Quaternion::operator * ( const Quaternion& _q ) const
{
     return Quaternion ( w * _q.x + x * _q.w + y * _q.z - z * _q.y,
                         w * _q.y + y * _q.w + z * _q.x - x * _q.z,
                         w * _q.z + z * _q.w + x * _q.y - y * _q.x,
                         w * _q.w - x * _q.x - y * _q.y - z * _q.z );
}
//----------------------------------------------------------------------------//
inline Quaternion Quaternion::operator * ( float _s ) const
{
     return Quaternion( x * _s, y * _s, z * _s, w * _s );
}
//----------------------------------------------------------------------------//
inline Quaternion operator * ( float _s, const Quaternion& _q )
{
     return Quaternion( _s * _q.x, _s * _q.y, _s * _q.z, _s * _q.w );
}
//----------------------------------------------------------------------------//
inline Quaternion Quaternion::operator - ( void ) const
{
     return Quaternion( -x, -y, -z, -w );
}
//----------------------------------------------------------------------------//
inline const Quaternion& Quaternion::operator + ( void ) const
{
     return *this;
}
//----------------------------------------------------------------------------//
inline float Quaternion::Dot( const Quaternion& _q ) const
{
     return ( x * _q.x + y * _q.y + z * _q.z + w * _q.w );
}
//----------------------------------------------------------------------------//
inline Quaternion& Quaternion::Inverse( void )
{
     float _norm = Norm();

     if( _norm > 0.0f )
     {
          float _invnorm = 1.0f / _norm;
          return Set( -x * _invnorm, -y * _invnorm, -z * _invnorm, w * _invnorm );
     }

     return SetZero();
}
//----------------------------------------------------------------------------//
inline Quaternion Quaternion::InverseCopy( void ) const
{
     return Quaternion( *this ).Inverse();
}
//----------------------------------------------------------------------------//
inline float Quaternion::Norm( void ) const
{
     return ( x * x + y * y + z * z + w * w );
}
//----------------------------------------------------------------------------//
inline Quaternion Quaternion::UnitInverse( void ) const
{
     return Quaternion( -x, -y, -z, w );
}
//----------------------------------------------------------------------------//
inline Quaternion Quaternion::Exp( void ) const
{
     float _angle = Math::Sqrt( x * x + y * y + z * z );
     Quaternion _exp( x, y, z );
     float _s; 
      
     Math::SinCos( _angle, _s, _exp.w );

     if( Math::Abs( _s ) >= Math::Epsilon )
     {
          float _coeff = _s / _angle;
          _exp.x *= _coeff;
          _exp.y *= _coeff;
          _exp.z *= _coeff;
     }
      
     return _exp;
}
//----------------------------------------------------------------------------//
inline Quaternion Quaternion::Log( void ) const
{
     Quaternion _log( x, y, z, 0.0f );

     if( Math::Abs( w ) < 1.0f )
     {
          float _angle = Math::ACos( w );
          float _s = Math::Sin( _angle );

          if( Math::Abs( _s ) >= Math::Epsilon )
          {
               float _coeff = _angle / _s;

               _log.x *= _coeff;
               _log.y *= _coeff;
               _log.z *= _coeff;
          }
     }

     return _log;
}
//----------------------------------------------------------------------------//
inline Vector3 Quaternion::operator * ( const Vector3& _v ) const
{
     Vector3 _uv, _uuv;
     Vector3 _qvec( x, y, z );

     _uv = _qvec.Cross( _v );
     _uuv = _qvec.Cross( _uv );
     _uv *= ( 2.0f * w );
     _uuv *= 2.0f;

     return ( _v + _uv + _uuv );
}
//----------------------------------------------------------------------------//
inline Vector3 operator * ( const Vector3& _v, const Quaternion& _q )
{
     return _q * _v;
}
//----------------------------------------------------------------------------//
inline Quaternion& Quaternion::Normalize( void )
{
     return ( (*this) = (*this) * Math::RSqrt( Norm() ) );    
}
//----------------------------------------------------------------------------//
inline Quaternion Quaternion::NormalizedCopy( void )
{
     return Quaternion( *this ).Normalize();
}
//----------------------------------------------------------------------------//
inline Quaternion& Quaternion::FromRotationMatrix( const Matrix3& _m )
{
     float _root, _trace( _m.m00 + _m.m11 + _m.m22 );

     if( _trace > 0.0f )
     {
          _root = Math::Sqrt( _trace + 1.0f );
          w = _root * 0.5f;
          _root = 0.5f / _root;
          x = ( _m.m21 - _m.m12 ) * _root;
          y = ( _m.m02 - _m.m20 ) * _root;
          z = ( _m.m10 - _m.m01 ) * _root;
     }
     else
     {
          if( _m.m00 > _m.m11 && _m.m00 > _m.m22 )
          {
               _root = Math::Sqrt( _m.m00 - _m.m11 - _m.m22 + 1.0f );
               x = _root * 0.5f;
               _root = 0.5f / _root;
               w = ( _m.m21 - _m.m12 ) * _root;
               y = ( _m.m10 + _m.m01 ) * _root;
               z = ( _m.m20 + _m.m02 ) * _root;
          }
          else if( _m.m11 > _m.m00 && _m.m11 > _m.m22 )
          {
               _root = Math::Sqrt( _m.m11 - _m.m22 - _m.m00 + 1.0f );
               y = _root * 0.5f;
               _root = 0.5f / _root;
               w = ( _m.m02 - _m.m20 ) * _root;
               z = ( _m.m21 + _m.m12 ) * _root;
               x = ( _m.m01 + _m.m10 ) * _root;
          }
          else
          {
               _root = Math::Sqrt( _m.m22 - _m.m00 - _m.m11 + 1.0f );
               z = _root * 0.5f;
               _root = 0.5f / _root;
               w = ( _m.m10 - _m.m01 ) * _root;
               x = ( _m.m02 + _m.m20 ) * _root;
               y = ( _m.m12 + _m.m21 ) * _root;
          }
     }

     return *this;
}
//----------------------------------------------------------------------------//
inline Matrix3& Quaternion::ToRotationMatrix( Matrix3& _m ) const
{
     float _x2 = x + x;
     float _y2 = y + y;
     float _z2 = z + z;
     float _wx = _x2 * w;
     float _wy = _y2 * w;
     float _wz = _z2 * w;
     float _xx = _x2 * x;
     float _xy = _y2 * x;
     float _xz = _z2 * x;
     float _yy = _y2 * y;
     float _yz = _z2 * y;
     float _zz = _z2 * z;

     return _m.Set( 1.0f - (_yy + _zz),  _xy - _wz,             _xz + _wy,
                    _xy + _wz,           1.0f - ( _xx + _zz ),  _yz - _wx,
                    _xz - _wy,           _yz + _wx,             1.0f - ( _xx + _yy ) );
}
//----------------------------------------------------------------------------//
inline Matrix3 Quaternion::ToRotationMatrix( void ) const
{
     return ToRotationMatrix( Matrix3() );
}
//----------------------------------------------------------------------------//
inline Quaternion& Quaternion::FromAxisAngle( const Vector3& _axis, float _angle )
{
     float _cs;
     Math::SinCos( _angle * 0.5f, _cs, w );
     x = _axis.x * _cs;
     y = _axis.y * _cs;
     z = _axis.z * _cs;

     return *this;
}
//----------------------------------------------------------------------------//
inline void Quaternion::ToAngleAxis( Vector3& _axis, float&_angle ) const
{
     float _sqrl( x * x + y * y + z * z );

     if( _sqrl > 0.0f )
     {
          _angle = Math::ACos( w ) * 2.0f;
          float _invl = Math::RSqrt( _sqrl );
          _axis.x = x * _invl;
          _axis.y = y * _invl;
          _axis.z = z * _invl;
     }
     else
     {
          _angle = 0.0f;
          _axis.x = 1.0f;
          _axis.y = 0.0f;
          _axis.z = 0.0f;
     }
}
//----------------------------------------------------------------------------//
inline float Quaternion::Roll( bool _reproject_axis ) const
{
     if( _reproject_axis )
     {
          float _y = 2.0f * y;
          float _z = 2.0f * z;
          float _wz = _z * w;
          float _xy = _y * x;
          float _yy = _y * y;
          float _zz = _z * z;

          return Math::ATan2( _xy + _wz, 1.0f - ( _yy + _zz ) );
     }
     else
     {
          return Math::ATan2( ( x * y + w * z ) * 2.0f, w * w + x * x - y * y - z * z );
     }
}
//----------------------------------------------------------------------------//
inline float Quaternion::Pitch( bool _reproject_axis ) const
{
     if( _reproject_axis )
     {
          float _x = 2.0f * x;
          float _z = 2.0f * z;
          float _wx = _x * w;
          float _xx = _x * x;
          float _yz = _z * y;
          float _zz = _z * z;

          return Math::ATan2( _yz + _wx, 1.0f - ( _xx + _zz ) );
     }
     else
     {
          return Math::ATan2( ( y * z + w * x ) * 2.0f, w * w - x * x - y * y + z * z );
     }
}
//----------------------------------------------------------------------------//
inline float Quaternion::Yaw( bool _reproject_axis ) const
{
     if( _reproject_axis )
     {
          float _x = 2.0f * x;
          float _y = 2.0f * y;
          float _z = 2.0f * z;
          float _wy = _y * w;
          float _xx = _x * x;
          float _xz = _z * x;
          float _yy = _y * y;

          return Math::ATan2( _xz + _wy, 1.0f - ( _xx + _yy ) );
     }
     else
     {
          return Math::ASin( ( x * z - w * y ) * -2.0f );
     }
}
//----------------------------------------------------------------------------//
inline Vector3 Quaternion::AxisX( void ) const
{
     float _y  = 2.0f * y;
     float _z  = 2.0f * z;
     float _wy = _y * w;
     float _wz = _z * w;
     float _xy = _y * x;
     float _xz = _z * x;
     float _yy = _y * y;
     float _zz = _z * z;

     return Vector3( 1.0f - ( _yy + _zz ), _xy + _wz, _xz - _wy );
}
//----------------------------------------------------------------------------//
inline Vector3 Quaternion::AxisY( void ) const
{
     float _x = 2.0f * x;
     float _y = 2.0f * y;
     float _z = 2.0f * z;
     float _wx = _x * w;
     float _wz = _z * w;
     float _xx = _x * x;
     float _xy = _y * x;
     float _yz = _z * y;
     float _zz = _z * z;

     return Vector3( _xy - _wz, 1.0f - ( _xx + _zz ), _yz + _wx );
}
//----------------------------------------------------------------------------//
inline Vector3 Quaternion::AxisZ( void ) const
{
     float _x  = 2.0f * x;
     float _y  = 2.0f * y;
     float _z  = 2.0f * z;
     float _wx = _x * w;
     float _wy = _y * w;
     float _xx = _x * x;
     float _xz = _z * x;
     float _yy = _y * y;
     float _yz = _z * y;

     return Vector3( _xz + _wy, _yz - _wx, 1.0f - ( _xx + _yy ) );
}
//----------------------------------------------------------------------------//
inline bool Quaternion::operator == ( const Quaternion& _q ) const
{
     return _q.x == x && _q.y == y && _q.z == z && _q.w == w;
}
//----------------------------------------------------------------------------//
inline bool Quaternion::operator != ( const Quaternion& _q ) const
{
     return !(*this == _q);
}
//----------------------------------------------------------------------------//
inline Quaternion& Quaternion::FromAxes( const Vector3& _x, const Vector3& _y, const Vector3& _z )
{
     return FromRotationMatrix( Matrix3().FromAxes( _x, _y, _z ) );
}
//----------------------------------------------------------------------------//
inline void Quaternion::ToAxes( Vector3& _x, Vector3& _y, Vector3& _z ) const
{
     ToRotationMatrix().ToAxes( _x, _y, _z );
}
//----------------------------------------------------------------------------//
inline bool Quaternion::Equals( const Quaternion& _q, float _tolerance ) const
{
     float _angle = Math::ACos( Dot( _q ) );

     return ( Math::Abs( _angle ) <= _tolerance ) || Math::Equals( _angle, Math::Pi, _tolerance );
}
//----------------------------------------------------------------------------//
inline Quaternion& Quaternion::Set( const Quaternion& _q )
{
     x = _q.x;
     y = _q.y;
     z = _q.z;
     w = _q.w;
     return *this;
}
//----------------------------------------------------------------------------//
inline Quaternion& Quaternion::Set( float _x, float _y, float _z, float _w )
{
     x = _x;
     y = _y;
     z = _z;
     w = _w;
     return *this;
}
//----------------------------------------------------------------------------//
inline Quaternion& Quaternion::FromPtr( const float _v[4] )
{
     x = _v[0];
     y = _v[1];
     z = _v[2];
     w = _v[3];
     return *this;
}
//----------------------------------------------------------------------------//
inline Quaternion& Quaternion::SetZero( void )
{
     return Set( 0.0f, 0.0f, 0.0f, 0.0f );
}
//----------------------------------------------------------------------------//
inline Quaternion& Quaternion::SetIdentity( void )
{
     return Set( 0.0f, 0.0f, 0.0f, 1.0f );
}
//----------------------------------------------------------------------------//
inline Quaternion Quaternion::Nlerp( float _t, const Quaternion& _p, const Quaternion& _q, bool _shortest_path )
{
     float _c = _p.Dot( _q );
     Quaternion _result;

     if( _c < 0.0f && _shortest_path ) _result = _p + _t * ( ( -_q ) - _p );
     else _result = _p + _t * ( _q - _p );

     return _result.Normalize();
}
//----------------------------------------------------------------------------//
inline Quaternion Quaternion::Slerp( float _t, const Quaternion& _p, const Quaternion& _q, bool _shortest_path )
{
     float _c = _p.Dot( _q );
     Quaternion _tmp;

     if( _c < 0.0f && _shortest_path )
     {
         _c = -_c;
         _tmp = -_q;
     }
     else
     {
         _tmp = _q;
     }

     if( Math::Abs( _c ) < 1.0f - Math::Epsilon )
     {
          float _s = Math::Sqrt( 1.0f - ( _c * _c ) );
          float _angle = Math::ATan2( _s, _c );
          float _invs = 1.0f / _s;
          float _coeff0 = Math::Sin( ( 1.0f - _t ) * _angle ) * _invs;
          float _coeff1 = Math::Sin( _t * _angle ) * _invs;

          return _coeff0 * _p + _coeff1 * _tmp;
     }
     else
     {
          return Quaternion( (1.0f - _t) * _p + _t * _tmp ).Normalize();
     }
}
//----------------------------------------------------------------------------//
inline Quaternion Quaternion::SlerpExtraSpins( float _t, const Quaternion& _p, const Quaternion& _q, int _extra_spins )
{
     float _c = _p.Dot( _q );
     float _angle = Math::ACos( _c );

     if ( Math::Abs( _angle ) < Math::Epsilon ) return _p;

     float _s = Math::Sin( _angle );
     float _phase = Math::Pi * _extra_spins * _t;
     float _invs = 1.0f / _s;
     float _coeff0 = Math::Sin( (1.0f - _t) * _angle - _phase ) * _invs;
     float _coeff1 = Math::Sin( _t * _angle + _phase ) * _invs;

     return _coeff0 * _p + _coeff1 * _q;
}
//----------------------------------------------------------------------------//
inline void Quaternion::Intermediate( const Quaternion& _q0, const Quaternion& _q1, const Quaternion& _q2, Quaternion& _a, Quaternion& _b )
{
     Quaternion _q0inv = _q0.UnitInverse();
     Quaternion _q1inv = _q1.UnitInverse();
     Quaternion _p0 = _q0inv * _q1;
     Quaternion _p1 = _q1inv * _q2;
     Quaternion _arg = 0.25f * ( _p0.Log() - _p1.Log() );
     Quaternion _minus_arg = -_arg;

     _a = _q1 * _arg.Exp();
     _b = _q1 * _minus_arg.Exp();
}
//----------------------------------------------------------------------------//
inline Quaternion Quaternion::Squad( float _t, const Quaternion& _p, const Quaternion& _a, const Quaternion& _b, const Quaternion& _q, bool _shortest_path )
{
     float _slerp_t = 2.0f * _t * ( 1.0f - _t );
     Quaternion _slerp_p = Slerp( _t, _p, _q, _shortest_path );
     Quaternion _slerp_q = Slerp( _t, _a, _b );

     return Slerp( _slerp_t, _slerp_p ,_slerp_q );
}
//----------------------------------------------------------------------------//

} // namespace ge

#endif//__Quaternion_hpp__